
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps_ublox.c
  * @author     baiyang
  * @date       2022-2-19
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <assert.h>

#include "sensor_gps_ublox.h"

#include <logger/ap_log.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/
#if MB_GPS_UBLOX_ENABLED

#define UBLOX_SPEED_CHANGE 0

#define UBLOX_DEBUGGING 0

#define UBLOX_FAKE_3DLOCK 0
#ifndef CONFIGURE_PPS_PIN
#define CONFIGURE_PPS_PIN 0
#endif

// this is number of epochs per output. A higher value will reduce
// the uart bandwidth needed and allow for higher latency
#define RTK_MB_RTCM_RATE 1

// use this to enable debugging of moving baseline configs
#define UBLOX_MB_DEBUGGING 0

#if UBLOX_DEBUGGING
#define UBLOX_DEBUG(fmt, args ...)  do {console_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define UBLOX_DEBUG(fmt, args ...)
#endif

#if UBLOX_MB_DEBUGGING
#define MB_Debug(fmt, args ...)  do {console_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define MB_Debug(fmt, args ...)
#endif

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_gps_ublox_destructor(sensor_gps_backend_t gps_backend);
static enum GPS_Status highest_supported_status(sensor_gps_backend_t gps_backend);
static bool is_configured(sensor_gps_backend_t gps_backend);
static void broadcast_configuration_failure_reason(sensor_gps_backend_t gps_backend);
static void Write_AP_Logger_Log_Startup_messages(sensor_gps_backend_t gps_backend);
static bool get_lag(sensor_gps_backend_t gps_backend, float *lag_sec);
static const char *name();
static bool get_RTCMV3(sensor_gps_backend_t gps_backend, const uint8_t *bytes, uint16_t *len);
static void clear_RTCMV3(sensor_gps_backend_t gps_backend);
static bool is_healthy(sensor_gps_backend_t gps_backend);

static bool read(sensor_gps_backend_t gps_backend);
static bool _parse_gps(sensor_gps_ublox_t gps_ublox);
static void _request_next_config(sensor_gps_ublox_t gps_ublox);
static bool _send_message(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
static void _update_checksum(uint8_t *data, uint16_t len, uint8_t *ck_a, uint8_t *ck_b);
static bool _request_message_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id);
static void _request_port(sensor_gps_ublox_t gps_ublox);
static void _request_version(sensor_gps_ublox_t gps_ublox);
static void _configure_rate(sensor_gps_ublox_t gps_ublox);
static bool supports_F9_config(sensor_gps_ublox_t gps_ublox);
static bool _configure_valset(sensor_gps_ublox_t gps_ublox, enum ConfigKey key, const void *value, uint8_t layers);
static bool _configure_valget(sensor_gps_ublox_t gps_ublox, enum ConfigKey key);
static bool _configure_config_set(sensor_gps_ublox_t gps_ublox, const struct config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers);
static void _save_cfg(sensor_gps_ublox_t gps_ublox);
static void _verify_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, uint8_t rate);
static bool _configure_message_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, uint8_t rate);
static uint8_t config_key_size(enum ConfigKey key);
static void log_mon_hw(sensor_gps_ublox_t gps_ublox);
static void log_mon_hw2(sensor_gps_ublox_t gps_ublox);
static void unexpected_message(sensor_gps_ublox_t gps_ublox);
static void log_rxm_raw(const struct ubx_rxm_raw *raw);
static void log_rxm_rawx(const struct ubx_rxm_rawx *raw);
static void _check_new_itow(sensor_gps_ublox_t gps_ublox, uint32_t itow);
static int8_t find_active_config_index(sensor_gps_ublox_t gps_ublox, enum ConfigKey key);
/*----------------------------------variable----------------------------------*/
static struct sensor_gps_backend_ops gps_ublox_ops = {.gps_backend_destructor = sensor_gps_ublox_destructor,
                                                        .read                   = read,
                                                        .highest_supported_status = highest_supported_status,
                                                        .is_configured          = is_configured,
                                                        .inject_data            = NULL,
                                                        .supports_mavlink_gps_rtk_message = NULL,
                                                        .send_mavlink_gps_rtk   = NULL,
                                                        .broadcast_configuration_failure_reason = broadcast_configuration_failure_reason,
                                                        .handle_msg             = NULL,
                                                        .get_lag                = get_lag,
                                                        .is_healthy             = is_healthy,
                                                        .logging_healthy        = NULL,
                                                        .name                   = name,
                                                        .Write_AP_Logger_Log_Startup_messages = Write_AP_Logger_Log_Startup_messages,
                                                        .prepare_for_arming     = NULL,
                                                        .get_RTCMV3             = get_RTCMV3,
                                                        .clear_RTCMV3           = clear_RTCMV3,
                                                        .get_error_codes        = NULL};

static const char *reasons[] = {"navigation rate",
                            "posllh rate",
                            "status rate",
                            "solution rate",
                            "velned rate",
                            "dop rate",
                            "hw monitor rate",
                            "hw2 monitor rate",
                            "raw rate",
                            "version",
                            "navigation settings",
                            "GNSS settings",
                            "SBAS settings",
                            "PVT rate",
                            "time pulse settings",
                            "TIMEGPS rate",
                            "Time mode settings",
                            "RTK MB",
                            "TIM TM2",
                            "M10"};

_Static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");

/*
  config changes for M10
  we need to use B1C not B1 signal for Beidou on M10 to allow solid 5Hz,
  and also disable Glonass and enable QZSS
 */
const struct config_list config_M10[] = {
    { CFG_SIGNAL_BDS_ENA, 1},
    { CFG_SIGNAL_BDS_B1_ENA, 0},
    { CFG_SIGNAL_BDS_B1C_ENA, 1},
    { CFG_SIGNAL_GLO_ENA, 0},
    { CFG_SIGNAL_QZSS_ENA, 1},
    { CFG_SIGNAL_QZSS_L1CA_ENA, 1},
    { CFG_SIGNAL_QZSS_L1S_ENA, 1},
    { CFG_NAVSPG_DYNMODEL, 8}, // Air < 4g
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_gps_ublox_ctor(sensor_gps_ublox_t gps_ublox, struct GPS_State *_state, rt_device_t _port,  enum GPS_Role _role)
{
    // 清空sensor_gps_ublox结构体变量，因为sensor_gps_ublox结构体有可能是申请的动态内存
    // 防止sensor_gps_ublox中的变量初始为非零值。
    rt_memset(gps_ublox, 0, sizeof(struct sensor_gps_ublox));

    sensor_gps_backend_ctor(&gps_ublox->backend, &gps_ublox_ops, _state, _port);

    gps_ublox->_next_message = STEP_PVT;
    gps_ublox->_ublox_port = 255;
    gps_ublox->_unconfigured_messages = CONFIG_ALL;
    gps_ublox->_hardware_generation = UBLOX_UNKNOWN_HARDWARE_GENERATION;
    gps_ublox->next_fix = NO_FIX;
    gps_ublox->noReceivedHdop = true;
    gps_ublox->role = _role;

    // stop any config strings that are pending
    sensor_gps_send_blob_start(gps_ublox->backend.state->instance, NULL, 0);

    // start the process of updating the GPS rates
    _request_next_config(gps_ublox);

#if CONFIGURE_PPS_PIN
    gps_ublox->_unconfigured_messages |= CONFIG_TP5;
#endif

#if GPS_MOVING_BASELINE

#endif
}

/*
  detect a Ublox GPS. Adds one byte, and returns true if the stream
  matches a UBlox
 */
bool sensor_gps_ublox_detect(struct UBLOX_detect_state *state, uint8_t data)
{
reset:
{
    switch (state->step) {
        case 1:
            if (PREAMBLE2 == data) {
                state->step++;
                break;
            }
            state->step = 0;
        case 0:
            if (PREAMBLE1 == data)
                state->step++;
            break;
        case 2:
            state->step++;
            state->ck_b = state->ck_a = data;
            break;
        case 3:
            state->step++;
            state->ck_b += (state->ck_a += data);
            break;
        case 4:
            state->step++;
            state->ck_b += (state->ck_a += data);
            state->payload_length = data;
            break;
        case 5:
            state->step++;
            state->ck_b += (state->ck_a += data);
            state->payload_counter = 0;
            break;
        case 6:
            state->ck_b += (state->ck_a += data);
            if (++state->payload_counter == state->payload_length)
                state->step++;
            break;
        case 7:
            state->step++;
            if (state->ck_a != data) {
                state->step = 0;
                goto reset;
            }
            break;
        case 8:
            state->step = 0;
            if (state->ck_b == data) {
                // a valid UBlox packet
                return true;
            } else {
                goto reset;
            }
    }
}
    return false;
}

sensor_gps_backend_t sensor_gps_ublox_probe(struct GPS_State *_state, rt_device_t _port, enum GPS_Role _role)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)rt_malloc(sizeof(struct sensor_gps_ublox));

    if (gps_ublox != NULL) {
        sensor_gps_ublox_ctor(gps_ublox, _state, _port, _role);
        return (sensor_gps_backend_t)gps_ublox;
    }

    return NULL;
}

static void sensor_gps_ublox_destructor(sensor_gps_backend_t gps_backend)
{
#if GPS_MOVING_BASELINE
    
#endif
}

static enum GPS_Status highest_supported_status(sensor_gps_backend_t gps_backend)
{
    return GPS_OK_FIX_3D_RTK_FIXED;
}

static bool is_configured(sensor_gps_backend_t gps_backend)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)gps_backend;

    if (!gps._auto_config) {
        return true;
    } else {
        return !gps_ublox->_unconfigured_messages;
    }
}

static void broadcast_configuration_failure_reason(sensor_gps_backend_t gps_backend)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)gps_backend;

    for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) {
        if (gps_ublox->_unconfigured_messages & (1 << i)) {
            gcs_send_text(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
                (unsigned int)(gps_ublox->backend.state->instance + 1), reasons[i], (unsigned int)gps_ublox->_unconfigured_messages);
            break;
        }
    }
}

static void Write_AP_Logger_Log_Startup_messages(sensor_gps_backend_t gps_backend)
{
#if HAL_LOGGING_ENABLED
    
#endif
}

/*
  return velocity lag in seconds
 */
static bool get_lag(sensor_gps_backend_t gps_backend, float *lag_sec)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)gps_backend;

    switch (gps_ublox->_hardware_generation) {
    case UBLOX_UNKNOWN_HARDWARE_GENERATION:
        *lag_sec = 0.22f;
        // always bail out in this case, it's used to indicate we have yet to receive a valid
        // hardware generation, however the user may have inhibited us detecting the generation
        // so if we aren't allowed to do configuration, we will accept this as the default delay
        return gps._auto_config == GPS_AUTO_CONFIG_DISABLE;
    case UBLOX_5:
    case UBLOX_6:
    default:
        *lag_sec = 0.22f;
        break;
    case UBLOX_7:
    case UBLOX_M8:
        // based on flight logs the 7 and 8 series seem to produce about 120ms lag
        *lag_sec = 0.12f;
        break;
    case UBLOX_F9:
    case UBLOX_M9:
        // F9 lag not verified yet from flight log, but likely to be at least
        // as good as M8
        *lag_sec = 0.12f;
#if GPS_MOVING_BASELINE
        if (gps_ublox->role == GPS_ROLE_MB_BASE ||
            gps_ublox->role == GPS_ROLE_MB_ROVER) {
            // the moving baseline rover will lag about 40ms from the
            // base. We need to provide the more conservative value to
            // ensure that the EKF allocates a larger enough buffer
            *lag_sec += 0.04;
        }
#endif
        break;
    };
    return true;
}

static const char *name() { return "u-blox"; }

// support for retrieving RTCMv3 data from a moving baseline base
static bool get_RTCMV3(sensor_gps_backend_t gps_backend, const uint8_t *bytes, uint16_t *len)
{
#if GPS_MOVING_BASELINE
    return false;
#endif
    return false;
}

// clear previous RTCM3 packet
static void clear_RTCMV3(sensor_gps_backend_t gps_backend)
{
#if GPS_MOVING_BASELINE

#endif
}

// ublox specific healthy checks
static bool is_healthy(sensor_gps_backend_t gps_backend)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)gps_backend;

#if GPS_MOVING_BASELINE

#endif
    return true;
}

// Ensure there is enough space for the largest possible outgoing message
// Process bytes available from the stream
//
// The stream is assumed to contain only messages we recognise.  If it
// contains other messages, and those messages contain the preamble
// bytes, it is possible for this code to fail to synchronise to the
// stream immediately.  Without buffering the entire message and
// re-processing it from the top, this is unavoidable. The parser
// attempts to avoid this when possible.
//
static bool read(sensor_gps_backend_t gps_backend)
{
    sensor_gps_ublox_t gps_ublox = (sensor_gps_ublox_t)gps_backend;

    bool parsed = false;
    uint32_t millis_now = time_millis();

    // walk through the gps configuration at 1 message per second
    if (millis_now - gps_ublox->_last_config_time >= gps_ublox->_delay_time) {
        _request_next_config(gps_ublox);
        gps_ublox->_last_config_time = millis_now;
        if (gps_ublox->_unconfigured_messages) { // send the updates faster until fully configured
            if (!gps_ublox->havePvtMsg && (gps_ublox->_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) {
                gps_ublox->_delay_time = 300;
            } else {
                gps_ublox->_delay_time = 750;
            }
        } else {
            gps_ublox->_delay_time = 2000;
        }
    }

    if(!gps_ublox->_unconfigured_messages && gps._save_config && !gps_ublox->_cfg_saved &&
       gps_ublox->_num_cfg_save_tries < 5 && (millis_now - gps_ublox->_last_cfg_sent_time) > 5000 &&
       !brd_get_soft_armed()) {
        //save the configuration sent until now
        if (gps._save_config == 1 ||
            (gps._save_config == 2 && gps_ublox->_cfg_needs_save)) {
            _save_cfg(gps_ublox);
        }
    }

    const uint16_t numc = MIN(SerialManager_rx_available(gps_ublox->backend.port), 8192U);
    for (uint16_t i = 0; i < numc; i++) {        // Process bytes received

        // read the next byte
        const int16_t rdata = SerialManager_read_byte(gps_ublox->backend.port);
        if (rdata < 0) {
            UBLOX_DEBUG("no rx date %u", __LINE__);
            break;
        }
        const uint8_t data = rdata;

#if GPS_MOVING_BASELINE

#endif

    reset:
        switch(gps_ublox->_step) {

        // Message preamble detection
        //
        // If we fail to match any of the expected bytes, we reset
        // the state machine and re-consider the failed byte as
        // the first byte of the preamble.  This improves our
        // chances of recovering from a mismatch and makes it less
        // likely that we will be fooled by the preamble appearing
        // as data in some other message.
        //
        case 1:
            if (PREAMBLE2 == data) {
                gps_ublox->_step++;
                break;
            }
            gps_ublox->_step = 0;
            UBLOX_DEBUG("reset %u", __LINE__);
        case 0:
            if(PREAMBLE1 == data)
                gps_ublox->_step++;
            break;

        // Message header processing
        //
        // We sniff the class and message ID to decide whether we
        // are going to gather the message bytes or just discard
        // them.
        //
        // We always collect the length so that we can avoid being
        // fooled by preamble bytes in messages.
        //
        case 2:
            gps_ublox->_step++;
            gps_ublox->_class = data;
            gps_ublox->_ck_b = gps_ublox->_ck_a = data;                       // reset the checksum accumulators
            break;
        case 3:
            gps_ublox->_step++;
            gps_ublox->_ck_b += (gps_ublox->_ck_a += data);                   // checksum byte
            gps_ublox->_msg_id = data;
            break;
        case 4:
            gps_ublox->_step++;
            gps_ublox->_ck_b += (gps_ublox->_ck_a += data);                   // checksum byte
            gps_ublox->_payload_length = data;                     // payload length low byte
            break;
        case 5:
            gps_ublox->_step++;
            gps_ublox->_ck_b += (gps_ublox->_ck_a += data);                   // checksum byte

            gps_ublox->_payload_length += (uint16_t)(data<<8);
            if (gps_ublox->_payload_length > sizeof(gps_ublox->_buffer)) {
                UBLOX_DEBUG("large payload %u", (unsigned)gps_ublox->_payload_length);
                // assume any payload bigger then what we know about is noise
                gps_ublox->_payload_length = 0;
                gps_ublox->_step = 0;
                goto reset;
            }
            gps_ublox->_payload_counter = 0;                       // prepare to receive payload
            if (gps_ublox->_payload_length == 0) {
                // bypass payload and go straight to checksum
                gps_ublox->_step++;
            }
            break;

        // Receive message data
        //
        case 6:
            gps_ublox->_ck_b += (gps_ublox->_ck_a += data);                   // checksum byte
            if (gps_ublox->_payload_counter < sizeof(gps_ublox->_buffer)) {
                ((uint8_t*)&gps_ublox->_buffer)[gps_ublox->_payload_counter] = data;
            }
            if (++gps_ublox->_payload_counter == gps_ublox->_payload_length)
                gps_ublox->_step++;
            break;

        // Checksum and message processing
        //
        case 7:
            gps_ublox->_step++;
            if (gps_ublox->_ck_a != data) {
                UBLOX_DEBUG("bad cka %x should be %x", data, gps_ublox->_ck_a);
                gps_ublox->_step = 0;
                aplog_write_message("ublox: ck_a failure");
                goto reset;
            }
            break;
        case 8:
            gps_ublox->_step = 0;
            if (gps_ublox->_ck_b != data) {
                UBLOX_DEBUG("bad ckb %x should be %x", data, gps_ublox->_ck_b);
                aplog_write_message("ublox: ck_b failure");
                break;                                                  // bad checksum
            }

#if GPS_MOVING_BASELINE

#endif
            if (_parse_gps(gps_ublox)) {
                parsed = true;
                UBLOX_DEBUG("parse_gps good");
            }
            break;
        }
    }

    return parsed;
}

static bool _parse_gps(sensor_gps_ublox_t gps_ublox)
{
    if (gps_ublox->_class == CLASS_ACK) {
        UBLOX_DEBUG("ACK %u", (unsigned)gps_ublox->_msg_id);
        UBLOX_DEBUG("clsId %u, msgID %u", (unsigned)gps_ublox->_buffer.ack.clsID,
                                          (unsigned)gps_ublox->_buffer.ack.msgID);

        if(gps_ublox->_msg_id == MSG_ACK_ACK) {
            switch(gps_ublox->_buffer.ack.clsID) {
            case CLASS_CFG:
                switch(gps_ublox->_buffer.ack.msgID) {
                case MSG_CFG_CFG:
                    gps_ublox->_cfg_saved = true;
                    gps_ublox->_cfg_needs_save = false;
                    break;
                case MSG_CFG_GNSS:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_GNSS;
                    break;
                case MSG_CFG_MSG:
                    // There is no way to know what MSG config was ack'ed, assume it was the last
                    // one requested. To verify it rerequest the last config we sent. If we miss
                    // the actual ack we will catch it next time through the poll loop, but that
                    // will be a good chunk of time later.
                    break;
                case MSG_CFG_NAV_SETTINGS:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
                    break;
                case MSG_CFG_RATE:
                    // The GPS will ACK a update rate that is invalid. in order to detect this
                    // only accept the rate as configured by reading the settings back and
                    // validating that they all match the target values
                    break;
                case MSG_CFG_SBAS:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_SBAS;
                    break;
                case MSG_CFG_TP5:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_TP5;
                    break;
                }
                break;
            case CLASS_MON:
                switch(gps_ublox->_buffer.ack.msgID) {
                case MSG_MON_HW:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
                    break;
                case MSG_MON_HW2:
                    gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
                    break;
                }
            }
        }

        if(gps_ublox->_msg_id == MSG_ACK_NACK) {
            switch(gps_ublox->_buffer.nack.clsID) {
            case CLASS_CFG:
                switch(gps_ublox->_buffer.nack.msgID) {
                case MSG_CFG_VALGET:
                    if (gps_ublox->active_config.list != NULL) {
                        /*
                          likely this device does not support fetching multiple keys at once, go one at a time
                        */
                        if (gps_ublox->active_config.fetch_index == -1) {
                            UBLOX_DEBUG("NACK starting %u", (unsigned)(gps_ublox->active_config.count));
                            gps_ublox->active_config.fetch_index = 0;
                        } else {
                            // the device does not support the config key we asked for,
                            // consider the bit as done
                            gps_ublox->active_config.done_mask |= (1U<<gps_ublox->active_config.fetch_index);
                            UBLOX_DEBUG("NACK %d 0x%x done=0x%x",
                                     (int)(gps_ublox->active_config.fetch_index),
                                     (unsigned)(gps_ublox->active_config.list[gps_ublox->active_config.fetch_index].key),
                                     (unsigned)(gps_ublox->active_config.done_mask));
                            if (gps_ublox->active_config.done_mask == (1U<<gps_ublox->active_config.count)-1) {
                                // all done!
                                gps_ublox->_unconfigured_messages &= ~gps_ublox->active_config.unconfig_bit;
                            }
                            gps_ublox->active_config.fetch_index++;
                        }
                        if (gps_ublox->active_config.fetch_index < gps_ublox->active_config.count) {
                            _configure_valget(gps_ublox, gps_ublox->active_config.list[gps_ublox->active_config.fetch_index].key);
                        }
                    }
                    break;
                }
            }
        }
        return false;
    }

    if (gps_ublox->_class == CLASS_CFG) {
        switch(gps_ublox->_msg_id) {
        case  MSG_CFG_NAV_SETTINGS:
        UBLOX_DEBUG("Got settings %u min_elev %d drLimit %u\n", 
                  (unsigned)gps_ublox->_buffer.nav_settings.dynModel,
                  (int)gps_ublox->_buffer.nav_settings.minElev,
                  (unsigned)gps_ublox->_buffer.nav_settings.drLimit);
            gps_ublox->_buffer.nav_settings.mask = 0;
            if (gps._navfilter != GPS_ENGINE_NONE &&
                gps_ublox->_buffer.nav_settings.dynModel != gps._navfilter) {
                // we've received the current nav settings, change the engine
                // settings and send them back
                UBLOX_DEBUG("Changing engine setting from %u to %u\n",
                      (unsigned)gps_ublox->_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
                gps_ublox->_buffer.nav_settings.dynModel = gps._navfilter;
                gps_ublox->_buffer.nav_settings.mask |= 1;
            }
            if (gps._min_elevation != -100 &&
                gps_ublox->_buffer.nav_settings.minElev != gps._min_elevation) {
                UBLOX_DEBUG("Changing min elevation to %d\n", (int)gps._min_elevation);
                gps_ublox->_buffer.nav_settings.minElev = gps._min_elevation;
                gps_ublox->_buffer.nav_settings.mask |= 2;
            }
            if (gps_ublox->_buffer.nav_settings.mask != 0) {
                _send_message(gps_ublox, CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                              &gps_ublox->_buffer.nav_settings,
                              sizeof(gps_ublox->_buffer.nav_settings));
                gps_ublox->_unconfigured_messages |= CONFIG_NAV_SETTINGS;
                gps_ublox->_cfg_needs_save = true;
            } else {
                gps_ublox->_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
            }
            return false;

#if UBLOX_GNSS_SETTINGS
        case MSG_CFG_GNSS:
            if (gps._gnss_mode[gps_ublox->backend.state->instance] != 0) {
                struct ubx_cfg_gnss start_gnss = gps_ublox->_buffer.gnss;
                uint8_t gnssCount = 0;
                UBLOX_DEBUG("Got GNSS Settings %u %u %u %u:\n",
                    (unsigned)gps_ublox->_buffer.gnss.msgVer,
                    (unsigned)gps_ublox->_buffer.gnss.numTrkChHw,
                    (unsigned)gps_ublox->_buffer.gnss.numTrkChUse,
                    (unsigned)gps_ublox->_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUGGING
                for(int i = 0; i < gps_ublox->_buffer.gnss.numConfigBlocks; i++) {
                    UBLOX_DEBUG("  %u %u %u 0x%08x\n",
                    (unsigned)gps_ublox->_buffer.gnss.configBlock[i].gnssId,
                    (unsigned)gps_ublox->_buffer.gnss.configBlock[i].resTrkCh,
                    (unsigned)gps_ublox->_buffer.gnss.configBlock[i].maxTrkCh,
                    (unsigned)gps_ublox->_buffer.gnss.configBlock[i].flags);
                }
#endif

                for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
                    if((gps._gnss_mode[gps_ublox->backend.state->instance] & (1 << i)) && i != GNSS_SBAS) {
                        gnssCount++;
                    }
                }
                for(int i = 0; i < gps_ublox->_buffer.gnss.numConfigBlocks; i++) {
                    // Reserve an equal portion of channels for all enabled systems that supports it
                    if(gps._gnss_mode[gps_ublox->backend.state->instance] & (1 << gps_ublox->_buffer.gnss.configBlock[i].gnssId)) {
                        if(GNSS_SBAS !=gps_ublox->_buffer.gnss.configBlock[i].gnssId && (gps_ublox->_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=gps_ublox->_buffer.gnss.configBlock[i].gnssId)) {
                            gps_ublox->_buffer.gnss.configBlock[i].resTrkCh = (gps_ublox->_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                            gps_ublox->_buffer.gnss.configBlock[i].maxTrkCh = gps_ublox->_buffer.gnss.numTrkChHw;
                        } else {
                            if(GNSS_SBAS ==gps_ublox->_buffer.gnss.configBlock[i].gnssId) {
                                gps_ublox->_buffer.gnss.configBlock[i].resTrkCh = 1;
                                gps_ublox->_buffer.gnss.configBlock[i].maxTrkCh = 3;
                            }
                            if(GNSS_GALILEO ==gps_ublox->_buffer.gnss.configBlock[i].gnssId) {
                                gps_ublox->_buffer.gnss.configBlock[i].resTrkCh = (gps_ublox->_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                                gps_ublox->_buffer.gnss.configBlock[i].maxTrkCh = 8; //Per the M8 receiver description UBX-13003221 - R16, 4.1.1.3 it is not recommended to set the number of galileo channels higher then eigh
                            }
                        }
                        gps_ublox->_buffer.gnss.configBlock[i].flags = gps_ublox->_buffer.gnss.configBlock[i].flags | 0x00000001;
                    } else {
                        gps_ublox->_buffer.gnss.configBlock[i].resTrkCh = 0;
                        gps_ublox->_buffer.gnss.configBlock[i].maxTrkCh = 0;
                        gps_ublox->_buffer.gnss.configBlock[i].flags = gps_ublox->_buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
                    }
                }
                if (rt_memcmp(&start_gnss, &gps_ublox->_buffer.gnss, sizeof(start_gnss))) {
                    _send_message(gps_ublox, CLASS_CFG, MSG_CFG_GNSS, &gps_ublox->_buffer.gnss, 4 + (8 * gps_ublox->_buffer.gnss.numConfigBlocks));
                    gps_ublox->_unconfigured_messages |= CONFIG_GNSS;
                    gps_ublox->_cfg_needs_save = true;
                } else {
                    gps_ublox->_unconfigured_messages &= ~CONFIG_GNSS;
                }
            } else {
                gps_ublox->_unconfigured_messages &= ~CONFIG_GNSS;
            }
            return false;
#endif

        case MSG_CFG_SBAS:
            if (gps._sbas_mode != SBAS_DoNotChange) {
            UBLOX_DEBUG("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
                      (unsigned)gps_ublox->_buffer.sbas.mode,
                      (unsigned)gps_ublox->_buffer.sbas.usage,
                      (unsigned)gps_ublox->_buffer.sbas.maxSBAS,
                      (unsigned)gps_ublox->_buffer.sbas.scanmode2,
                      (unsigned)gps_ublox->_buffer.sbas.scanmode1);
                if (gps_ublox->_buffer.sbas.mode != gps._sbas_mode) {
                    gps_ublox->_buffer.sbas.mode = gps._sbas_mode;
                    _send_message(gps_ublox, CLASS_CFG, MSG_CFG_SBAS,
                                  &gps_ublox->_buffer.sbas,
                                  sizeof(gps_ublox->_buffer.sbas));
                    gps_ublox->_unconfigured_messages |= CONFIG_SBAS;
                    gps_ublox->_cfg_needs_save = true;
                } else {
                    gps_ublox->_unconfigured_messages &= ~CONFIG_SBAS;
                }
            } else {
                    gps_ublox->_unconfigured_messages &= ~CONFIG_SBAS;
            }
            return false;
        case MSG_CFG_MSG:
            if(gps_ublox->_payload_length == sizeof(struct ubx_cfg_msg_rate_6)) {
                // can't verify the setting without knowing the port
                // request the port again
                if(gps_ublox->_ublox_port >= UBLOX_MAX_PORTS) {
                    _request_port(gps_ublox);
                    return false;
                }
                _verify_rate(gps_ublox, gps_ublox->_buffer.msg_rate_6.msg_class, gps_ublox->_buffer.msg_rate_6.msg_id,
                             gps_ublox->_buffer.msg_rate_6.rates[gps_ublox->_ublox_port]);
            } else {
                _verify_rate(gps_ublox, gps_ublox->_buffer.msg_rate.msg_class, gps_ublox->_buffer.msg_rate.msg_id,
                             gps_ublox->_buffer.msg_rate.rate);
            }
            return false;
        case MSG_CFG_PRT:
           gps_ublox->_ublox_port = gps_ublox->_buffer.prt.portID;
           UBLOX_DEBUG("Got PRT  %u\n", (unsigned)gps_ublox->_buffer.prt.portID);
           return false;
        case MSG_CFG_RATE:
            if(gps_ublox->_buffer.nav_rate.measure_rate_ms != gps._rate_ms[gps_ublox->backend.state->instance] ||
               gps_ublox->_buffer.nav_rate.nav_rate != 1 ||
               gps_ublox->_buffer.nav_rate.timeref != 0) {
                _configure_rate(gps_ublox);
                gps_ublox->_unconfigured_messages |= CONFIG_RATE_NAV;
                gps_ublox->_cfg_needs_save = true;
            } else {
                gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_NAV;
            }
            return false;
            
#if CONFIGURE_PPS_PIN
        case MSG_CFG_TP5: {
            // configure the PPS pin for 1Hz, zero delay
            UBLOX_DEBUG("Got TP5 ver=%u 0x%04x %u\n", 
                  (unsigned)gps_ublox->_buffer.nav_tp5.version,
                  (unsigned)gps_ublox->_buffer.nav_tp5.flags,
                  (unsigned)gps_ublox->_buffer.nav_tp5.freqPeriod);
            const uint16_t desired_flags = 0x003f;
            const uint16_t desired_period_hz = 1;
            if (gps_ublox->_buffer.nav_tp5.flags != desired_flags ||
                gps_ublox->_buffer.nav_tp5.freqPeriod != desired_period_hz) {
                gps_ublox->_buffer.nav_tp5.tpIdx = 0;
                gps_ublox->_buffer.nav_tp5.reserved1[0] = 0;
                gps_ublox->_buffer.nav_tp5.reserved1[1] = 0;
                gps_ublox->_buffer.nav_tp5.antCableDelay = 0;
                gps_ublox->_buffer.nav_tp5.rfGroupDelay = 0;
                gps_ublox->_buffer.nav_tp5.freqPeriod = desired_period_hz;
                gps_ublox->_buffer.nav_tp5.freqPeriodLock = desired_period_hz;
                gps_ublox->_buffer.nav_tp5.pulseLenRatio = 1;
                gps_ublox->_buffer.nav_tp5.pulseLenRatioLock = 2;
                gps_ublox->_buffer.nav_tp5.userConfigDelay = 0;
                gps_ublox->_buffer.nav_tp5.flags = desired_flags;
                _send_message(gps_ublox, CLASS_CFG, MSG_CFG_TP5,
                              &gps_ublox->_buffer.nav_tp5,
                              sizeof(gps_ublox->_buffer.nav_tp5));
                gps_ublox->_unconfigured_messages |= CONFIG_TP5;
                gps_ublox->_cfg_needs_save = true;
            } else {
                gps_ublox->_unconfigured_messages &= ~CONFIG_TP5;
            }
            return false;
        }
#endif // CONFIGURE_PPS_PIN
        case MSG_CFG_VALGET: {
            uint8_t cfg_len = gps_ublox->_payload_length - sizeof(struct ubx_cfg_valget);
            const uint8_t *cfg_data = (const uint8_t *)(&gps_ublox->_buffer) + sizeof(struct ubx_cfg_valget);
            while (cfg_len >= 5) {
                enum ConfigKey id;
                rt_memcpy(&id, cfg_data, sizeof(uint32_t));
                cfg_len -= 4;
                cfg_data += 4;
                switch (id) {
                    case TMODE_MODE: {
                        uint8_t mode = cfg_data[0];
                        if (mode != 0) {
                            // ask for mode 0, to disable TIME mode
                            mode = 0;
                            _configure_valset(gps_ublox, TMODE_MODE, &mode, UBX_VALSET_LAYER_ALL);
                            gps_ublox->_cfg_needs_save = true;
                            gps_ublox->_unconfigured_messages |= CONFIG_TMODE_MODE;
                        } else {
                            gps_ublox->_unconfigured_messages &= ~CONFIG_TMODE_MODE;
                        }
                        break;
                    }
                    default:
                        break;
                }

                // see if it is in active config list
                int8_t cfg_idx = find_active_config_index(gps_ublox, id);
                if (cfg_idx >= 0) {
                    const uint8_t key_size = config_key_size(id);
                    if (cfg_len < key_size ||
                        rt_memcmp(&gps_ublox->active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
                        _configure_valset(gps_ublox, id, &gps_ublox->active_config.list[cfg_idx].value, gps_ublox->active_config.layers);
                        gps_ublox->_unconfigured_messages |= gps_ublox->active_config.unconfig_bit;
                        gps_ublox->active_config.done_mask &= ~(1U << cfg_idx);
                        gps_ublox->_cfg_needs_save = true;
                    } else {
                        gps_ublox->active_config.done_mask |= (1U << cfg_idx);
                        UBLOX_DEBUG("done %u mask=0x%x",
                              (unsigned)(cfg_idx),
                              (unsigned)(gps_ublox->active_config.done_mask));
                        if (gps_ublox->active_config.done_mask == (1U<<gps_ublox->active_config.count)-1) {
                            // all done!
                            gps_ublox->_unconfigured_messages &= ~gps_ublox->active_config.unconfig_bit;
                        }
                    }
                    if (gps_ublox->active_config.fetch_index >= 0 &&
                        gps_ublox->active_config.fetch_index < gps_ublox->active_config.count &&
                        id == gps_ublox->active_config.list[gps_ublox->active_config.fetch_index].key) {
                        gps_ublox->active_config.fetch_index++;
                        if (gps_ublox->active_config.fetch_index < gps_ublox->active_config.count) {
                            _configure_valget(gps_ublox, gps_ublox->active_config.list[gps_ublox->active_config.fetch_index].key);
                            UBLOX_DEBUG("valget %d 0x%x", (int)(gps_ublox->active_config.fetch_index),
                                  (unsigned)(gps_ublox->active_config.list[gps_ublox->active_config.fetch_index].key));
                        }
                    }
                }

                // step over the value
                uint8_t step_size = config_key_size(id);
                if (step_size == 0) {
                    return false;
                }
                cfg_len -= step_size;
                cfg_data += step_size;
            }
        }
        }
    }

    if (gps_ublox->_class == CLASS_MON) {
        switch(gps_ublox->_msg_id) {
        case MSG_MON_HW:
            if (gps_ublox->_payload_length == 60 || gps_ublox->_payload_length == 68) {
                log_mon_hw(gps_ublox);
            }
            break;
        case MSG_MON_HW2:
            if (gps_ublox->_payload_length == 28) {
                log_mon_hw2(gps_ublox);  
            }
            break;
        case MSG_MON_VER:
            gps_ublox->_have_version = true;
            strncpy(gps_ublox->_version.hwVersion, gps_ublox->_buffer.mon_ver.hwVersion, sizeof(gps_ublox->_version.hwVersion));
            strncpy(gps_ublox->_version.swVersion, gps_ublox->_buffer.mon_ver.swVersion, sizeof(gps_ublox->_version.swVersion));
            gcs_send_text(MAV_SEVERITY_INFO, 
                                             "u-blox %d HW: %s SW: %s",
                                             gps_ublox->backend.state->instance + 1,
                                             gps_ublox->_version.hwVersion,
                                             gps_ublox->_version.swVersion);
            // check for F9 and M9. The F9 does not respond to SVINFO,
            // so we need to use MON_VER for hardware generation
            if (strncmp(gps_ublox->_version.hwVersion, "00190000", 8) == 0) {
                if (strncmp(gps_ublox->_version.swVersion, "EXT CORE 1", 10) == 0) {
                    // a F9
                    if (gps_ublox->_hardware_generation != UBLOX_F9) {
                        // need to ensure time mode is correctly setup on F9
                        gps_ublox->_unconfigured_messages |= CONFIG_TMODE_MODE;
                    }
                    gps_ublox->_hardware_generation = UBLOX_F9;
                }
                if (strncmp(gps_ublox->_version.swVersion, "EXT CORE 4", 10) == 0) {
                    // a M9
                    gps_ublox->_hardware_generation = UBLOX_M9;
                }
            }
            // check for M10
            if (strncmp(gps_ublox->_version.hwVersion, "000A0000", 8) == 0) {
                if (strncmp(gps_ublox->_version.swVersion, "ROM SPG 5", 9) == 0) {
                    gps_ublox->_hardware_generation = UBLOX_M10;
                    gps_ublox->_unconfigured_messages |= CONFIG_M10;
                }
            }
            break;
        default:
            unexpected_message(gps_ublox);
        }
        return false;
    }

#if UBLOX_RXM_RAW_LOGGING
    if (gps_ublox->_class == CLASS_RXM && gps_ublox->_msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
        log_rxm_raw(&gps_ublox->_buffer.rxm_raw);
        return false;
    } else if (gps_ublox->_class == CLASS_RXM && gps_ublox->_msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
        log_rxm_rawx(&gps_ublox->_buffer.rxm_rawx);
        return false;
    }
#endif // UBLOX_RXM_RAW_LOGGING

    if (gps_ublox->_class != CLASS_NAV) {
        unexpected_message(gps_ublox);
        return false;
    }

    switch (gps_ublox->_msg_id) {
    case MSG_POSLLH:
        UBLOX_DEBUG("MSG_POSLLH next_fix=%u", gps_ublox->next_fix);
        if (gps_ublox->havePvtMsg) {
            gps_ublox->_unconfigured_messages |= CONFIG_RATE_POSLLH;
            break;
        }
        _check_new_itow(gps_ublox, gps_ublox->_buffer.posllh.itow);
        gps_ublox->_last_pos_time        = gps_ublox->_buffer.posllh.itow;
        gps_ublox->backend.state->location.lng    = gps_ublox->_buffer.posllh.longitude;
        gps_ublox->backend.state->location.lat    = gps_ublox->_buffer.posllh.latitude;
        gps_ublox->backend.state->location.alt    = gps_ublox->_buffer.posllh.altitude_msl / 10;

        gps_ublox->backend.state->have_undulation = true;
        gps_ublox->backend.state->undulation = (gps_ublox->_buffer.posllh.altitude_msl - gps_ublox->_buffer.posllh.altitude_ellipsoid) * 0.001f;

        gps_ublox->backend.state->status          = gps_ublox->next_fix;
        gps_ublox->_new_position = true;
        gps_ublox->backend.state->horizontal_accuracy = gps_ublox->_buffer.posllh.horizontal_accuracy*1.0e-3f;
        gps_ublox->backend.state->vertical_accuracy = gps_ublox->_buffer.posllh.vertical_accuracy*1.0e-3f;
        gps_ublox->backend.state->have_horizontal_accuracy = true;
        gps_ublox->backend.state->have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
        gps_ublox->backend.state->location.lng = 1491652300L;
        gps_ublox->backend.state->location.lat = -353632610L;
        gps_ublox->backend.state->location.alt = 58400;
        gps_ublox->backend.state->vertical_accuracy = 0;
        gps_ublox->backend.state->horizontal_accuracy = 0;
#endif
        break;
    case MSG_STATUS:
        UBLOX_DEBUG("MSG_STATUS fix_status=%u fix_type=%u",
              gps_ublox->_buffer.status.fix_status,
              gps_ublox->_buffer.status.fix_type);
        _check_new_itow(gps_ublox, gps_ublox->_buffer.status.itow);
        if (gps_ublox->havePvtMsg) {
            gps_ublox->_unconfigured_messages |= CONFIG_RATE_STATUS;
            break;
        }
        if (gps_ublox->_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if ((gps_ublox->_buffer.status.fix_type == FIX_3D) &&
                (gps_ublox->_buffer.status.fix_status & NAV_STATUS_DGPS_USED)) {
                gps_ublox->next_fix = GPS_OK_FIX_3D_DGPS;
            }else if (gps_ublox->_buffer.status.fix_type == FIX_3D) {
                gps_ublox->next_fix = GPS_OK_FIX_3D;
            }else if (gps_ublox->_buffer.status.fix_type == FIX_2D) {
                gps_ublox->next_fix = GPS_OK_FIX_2D;
            }else{
                gps_ublox->next_fix = NO_FIX;
                gps_ublox->backend.state->status = NO_FIX;
            }
        }else{
            gps_ublox->next_fix = NO_FIX;
            gps_ublox->backend.state->status = NO_FIX;
        }
#if UBLOX_FAKE_3DLOCK
        gps_ublox->backend.state->status = GPS_OK_FIX_3D;
        gps_ublox->next_fix = gps_ublox->backend.state->status;
#endif
        break;
    case MSG_DOP:
        UBLOX_DEBUG("MSG_DOP");
        gps_ublox->noReceivedHdop = false;
        _check_new_itow(gps_ublox, gps_ublox->_buffer.dop.itow);
        gps_ublox->backend.state->hdop        = gps_ublox->_buffer.dop.hDOP;
        gps_ublox->backend.state->vdop        = gps_ublox->_buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
        gps_ublox->backend.state->hdop = 130;
        gps_ublox->backend.state->vdop = 170;
#endif
        break;
    case MSG_SOL:
        UBLOX_DEBUG("MSG_SOL fix_status=%u fix_type=%u",
              gps_ublox->_buffer.solution.fix_status,
              gps_ublox->_buffer.solution.fix_type);
        _check_new_itow(gps_ublox, gps_ublox->_buffer.solution.itow);
        if (gps_ublox->havePvtMsg) {
            gps_ublox->backend.state->time_week = gps_ublox->_buffer.solution.week;
            break;
        }
        if (gps_ublox->_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if ((gps_ublox->_buffer.solution.fix_type == FIX_3D) &&
                (gps_ublox->_buffer.solution.fix_status & NAV_STATUS_DGPS_USED)) {
                gps_ublox->next_fix = GPS_OK_FIX_3D_DGPS;
            }else if (gps_ublox->_buffer.solution.fix_type == FIX_3D) {
                gps_ublox->next_fix = GPS_OK_FIX_3D;
            }else if (gps_ublox->_buffer.solution.fix_type == FIX_2D) {
                gps_ublox->next_fix = GPS_OK_FIX_2D;
            }else{
                gps_ublox->next_fix = NO_FIX;
                gps_ublox->backend.state->status = NO_FIX;
            }
        }else{
            gps_ublox->next_fix = NO_FIX;
            gps_ublox->backend.state->status = NO_FIX;
        }
        if(gps_ublox->noReceivedHdop) {
            gps_ublox->backend.state->hdop = gps_ublox->_buffer.solution.position_DOP;
        }
        gps_ublox->backend.state->num_sats    = gps_ublox->_buffer.solution.satellites;
        if (gps_ublox->next_fix >= GPS_OK_FIX_2D) {
            gps_ublox->backend.state->last_gps_time_us = time_micros64();
            gps_ublox->backend.state->last_gps_time_ms = gps_ublox->backend.state->last_gps_time_us/1000;
            gps_ublox->backend.state->time_week_ms    = gps_ublox->_buffer.solution.itow;
            gps_ublox->backend.state->time_week       = gps_ublox->_buffer.solution.week;
        }
#if UBLOX_FAKE_3DLOCK
        gps_ublox->next_fix = gps_ublox->backend.state->status;
        gps_ublox->backend.state->num_sats = 10;
        gps_ublox->backend.state->time_week = 1721;
        gps_ublox->backend.state->time_week_ms = time_millis() + 3*60*60*1000 + 37000;
        gps_ublox->backend.state->last_gps_time_us = time_micros64();
        gps_ublox->backend.state->last_gps_time_ms = gps_ublox->backend.state->last_gps_time_us/1000;
        gps_ublox->backend.state->hdop = 130;
#endif
        break;

#if GPS_MOVING_BASELINE
    case MSG_RELPOSNED:
        {

        }
        break;
#endif // GPS_MOVING_BASELINE

    case MSG_PVT:
        UBLOX_DEBUG("MSG_PVT");

        gps_ublox->havePvtMsg = true;
        // position
        _check_new_itow(gps_ublox, gps_ublox->_buffer.pvt.itow);
        gps_ublox->_last_pvt_itow = gps_ublox->_buffer.pvt.itow;
        gps_ublox->_last_pos_time        = gps_ublox->_buffer.pvt.itow;
        gps_ublox->backend.state->location.lng    = gps_ublox->_buffer.pvt.lon;
        gps_ublox->backend.state->location.lat    = gps_ublox->_buffer.pvt.lat;
        gps_ublox->backend.state->location.alt    = gps_ublox->_buffer.pvt.h_msl / 10;

        gps_ublox->backend.state->have_undulation = true;
        gps_ublox->backend.state->undulation = (gps_ublox->_buffer.pvt.h_msl - gps_ublox->_buffer.pvt.h_ellipsoid) * 0.001;

        switch (gps_ublox->_buffer.pvt.fix_type) 
        {
            case 0:
                gps_ublox->backend.state->status = NO_FIX;
                break;
            case 1:
                gps_ublox->backend.state->status = NO_FIX;
                break;
            case 2:
                gps_ublox->backend.state->status = GPS_OK_FIX_2D;
                break;
            case 3:
                gps_ublox->backend.state->status = GPS_OK_FIX_3D;
                if (gps_ublox->_buffer.pvt.flags & 0b00000010)  // diffsoln
                    gps_ublox->backend.state->status = GPS_OK_FIX_3D_DGPS;
                if (gps_ublox->_buffer.pvt.flags & 0b01000000)  // carrsoln - float
                    gps_ublox->backend.state->status = GPS_OK_FIX_3D_RTK_FLOAT;
                if (gps_ublox->_buffer.pvt.flags & 0b10000000)  // carrsoln - fixed
                    gps_ublox->backend.state->status = GPS_OK_FIX_3D_RTK_FIXED;
                break;
            case 4:
                gcs_send_text(MAV_SEVERITY_INFO,
                                "Unexpected state %d", gps_ublox->_buffer.pvt.flags);
                gps_ublox->backend.state->status = GPS_OK_FIX_3D;
                break;
            case 5:
                gps_ublox->backend.state->status = NO_FIX;
                break;
            default:
                gps_ublox->backend.state->status = NO_FIX;
                break;
        }
        gps_ublox->next_fix = gps_ublox->backend.state->status;
        gps_ublox->_new_position = true;
        gps_ublox->backend.state->horizontal_accuracy = gps_ublox->_buffer.pvt.h_acc*1.0e-3f;
        gps_ublox->backend.state->vertical_accuracy = gps_ublox->_buffer.pvt.v_acc*1.0e-3f;
        gps_ublox->backend.state->have_horizontal_accuracy = true;
        gps_ublox->backend.state->have_vertical_accuracy = true;
        // SVs
        gps_ublox->backend.state->num_sats    = gps_ublox->_buffer.pvt.num_sv;
        // velocity     
        gps_ublox->_last_vel_time         = gps_ublox->_buffer.pvt.itow;
        gps_ublox->backend.state->ground_speed     = gps_ublox->_buffer.pvt.gspeed*0.001f;          // m/s
        gps_ublox->backend.state->ground_course    = math_wrap_360(gps_ublox->_buffer.pvt.head_mot * 1.0e-5f);       // Heading 2D deg * 100000
        gps_ublox->backend.state->have_horizontal_velocity = true;
        gps_ublox->backend.state->have_vertical_velocity = true;
        gps_ublox->backend.state->velocity.x = gps_ublox->_buffer.pvt.velN * 0.001f;
        gps_ublox->backend.state->velocity.y = gps_ublox->_buffer.pvt.velE * 0.001f;
        gps_ublox->backend.state->velocity.z = gps_ublox->_buffer.pvt.velD * 0.001f;
        gps_ublox->backend.state->have_speed_accuracy = true;
        gps_ublox->backend.state->speed_accuracy = gps_ublox->_buffer.pvt.s_acc*0.001f;
        gps_ublox->_new_speed = true;
        // dop
        if(gps_ublox->noReceivedHdop) {
            gps_ublox->backend.state->hdop        = gps_ublox->_buffer.pvt.p_dop;
            gps_ublox->backend.state->vdop        = gps_ublox->_buffer.pvt.p_dop;
        }

        gps_ublox->backend.state->last_gps_time_us = time_micros64();
        gps_ublox->backend.state->last_gps_time_ms = gps_ublox->backend.state->last_gps_time_us/1000;
        
        // time
        gps_ublox->backend.state->time_week_ms    = gps_ublox->_buffer.pvt.itow;
#if UBLOX_FAKE_3DLOCK
        gps_ublox->backend.state->location.lng = 1491652300L;
        gps_ublox->backend.state->location.lat = -353632610L;
        gps_ublox->backend.state->location.alt = 58400;
        gps_ublox->backend.state->vertical_accuracy = 0;
        gps_ublox->backend.state->horizontal_accuracy = 0;
        gps_ublox->backend.state->status = GPS_OK_FIX_3D;
        gps_ublox->backend.state->num_sats = 10;
        gps_ublox->backend.state->time_week = 1721;
        gps_ublox->backend.state->time_week_ms = time_millis() + 3*60*60*1000 + 37000;
        gps_ublox->backend.state->last_gps_time_us = time_micros64();
        gps_ublox->backend.state->last_gps_time_ms = gps_ublox->backend.state->last_gps_time_us/1000;
        gps_ublox->backend.state->hdop = 130;
        gps_ublox->backend.state->speed_accuracy = 0;
        gps_ublox->next_fix = gps_ublox->backend.state->status;
#endif
        break;
    case MSG_TIMEGPS:
        UBLOX_DEBUG("MSG_TIMEGPS");
        _check_new_itow(gps_ublox, gps_ublox->_buffer.timegps.itow);
        if (gps_ublox->_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) {
            gps_ublox->backend.state->time_week = gps_ublox->_buffer.timegps.week;
        }
        break;
    case MSG_VELNED:
        UBLOX_DEBUG("MSG_VELNED");
        if (gps_ublox->havePvtMsg) {
            gps_ublox->_unconfigured_messages |= CONFIG_RATE_VELNED;
            break;
        }
        _check_new_itow(gps_ublox, gps_ublox->_buffer.velned.itow);
        gps_ublox->_last_vel_time         = gps_ublox->_buffer.velned.itow;
        gps_ublox->backend.state->ground_speed     = gps_ublox->_buffer.velned.speed_2d*0.01f;          // m/s
        gps_ublox->backend.state->ground_course    = math_wrap_360(gps_ublox->_buffer.velned.heading_2d * 1.0e-5f);       // Heading 2D deg * 100000
        gps_ublox->backend.state->have_horizontal_velocity = true;
        gps_ublox->backend.state->have_vertical_velocity = true;
        gps_ublox->backend.state->velocity.x = gps_ublox->_buffer.velned.ned_north * 0.01f;
        gps_ublox->backend.state->velocity.y = gps_ublox->_buffer.velned.ned_east * 0.01f;
        gps_ublox->backend.state->velocity.z = gps_ublox->_buffer.velned.ned_down * 0.01f;
        gps_ublox->backend.state->ground_course = math_wrap_360(degrees(atan2f(gps_ublox->backend.state->velocity.y, gps_ublox->backend.state->velocity.x)));
        gps_ublox->backend.state->ground_speed = vec3_length_xy(&gps_ublox->backend.state->velocity);
        gps_ublox->backend.state->have_speed_accuracy = true;
        gps_ublox->backend.state->speed_accuracy = gps_ublox->_buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK
        gps_ublox->backend.state->speed_accuracy = 0;
#endif
        gps_ublox->_new_speed = true;
        break;
    case MSG_NAV_SVINFO:
        {
        UBLOX_DEBUG("MSG_NAV_SVINFO\n");
        static const uint8_t HardwareGenerationMask = 0x07;
        _check_new_itow(gps_ublox, gps_ublox->_buffer.svinfo_header.itow);
        gps_ublox->_hardware_generation = gps_ublox->_buffer.svinfo_header.globalFlags & HardwareGenerationMask;
        switch (gps_ublox->_hardware_generation) {
            case UBLOX_5:
            case UBLOX_6:
                // only 7 and newer support CONFIG_GNSS
                gps_ublox->_unconfigured_messages &= ~CONFIG_GNSS;
                break;
            case UBLOX_7:
            case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
                SerialManager_set_baudrate(gps_ublox->backend.port, 4000000U);
                UBLOX_DEBUG("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
                break;
            default:
                console_printf("Wrong Ublox Hardware Version%u\n", gps_ublox->_hardware_generation);
                break;
        };
        gps_ublox->_unconfigured_messages &= ~CONFIG_VERSION;
        /* We don't need that anymore */
        _configure_message_rate(gps_ublox, CLASS_NAV, MSG_NAV_SVINFO, 0);
        break;
        }
    default:
        UBLOX_DEBUG("Unexpected NAV message 0x%02x", (unsigned)gps_ublox->_msg_id);
        if (++gps_ublox->_disable_counter == 0) {
            UBLOX_DEBUG("Disabling NAV message 0x%02x", (unsigned)gps_ublox->_msg_id);
            _configure_message_rate(gps_ublox, CLASS_NAV, gps_ublox->_msg_id, 0);
        }
        return false;
    }

    if (gps_ublox->backend.state->have_gps_yaw) {
        // when we are a rover we want to ensure we have both the new
        // PVT and the new RELPOSNED message so that we give a
        // consistent view
        if (time_millis() - gps_ublox->_last_relposned_ms > 400) {
            // we have stopped receiving valid RELPOSNED messages, disable yaw reporting
            gps_ublox->backend.state->have_gps_yaw = false;
        } else if (gps_ublox->_last_relposned_itow != gps_ublox->_last_pvt_itow) {
            // wait until ITOW matches
            return false;
        }
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (gps_ublox->_new_position && gps_ublox->_new_speed && gps_ublox->_last_vel_time == gps_ublox->_last_pos_time) {
        gps_ublox->_new_speed = gps_ublox->_new_position = false;
        return true;
    }
    return false;
}

/**
  * @brief       
  * @param[in]   gps_ublox  
  * @param[out]  
  * @retval      
  * @note        
  */
static void _request_next_config(sensor_gps_ublox_t gps_ublox)
{
    // don't request config if we shouldn't configure the GPS
    if (gps._auto_config == GPS_AUTO_CONFIG_DISABLE) {
        return;
    }

    // Ensure there is enough space for the largest possible outgoing message
    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
        // not enough space - do it next time
        return;
    }

    if (gps_ublox->_unconfigured_messages == CONFIG_RATE_SOL && gps_ublox->havePvtMsg) {
        /*
          we don't need SOL if we have PVT and TIMEGPS. This is needed
          as F9P doesn't support the SOL message
         */
        gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_SOL;
    }

    UBLOX_DEBUG("Unconfigured messages: 0x%x Current message: %u\n", (unsigned)gps_ublox->_unconfigured_messages, (unsigned)gps_ublox->_next_message);

    // check AP_GPS_UBLOX.h for the enum that controls the order.
    // This switch statement isn't maintained against the enum in order to reduce code churn
    switch (gps_ublox->_next_message++) {
    case STEP_PVT:
        if(!_request_message_rate(gps_ublox, CLASS_NAV, MSG_PVT)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_TIMEGPS:
        if(!_request_message_rate(gps_ublox, CLASS_NAV, MSG_TIMEGPS)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_PORT:
        _request_port(gps_ublox);
        break;
    case STEP_POLL_SVINFO:
        // not required once we know what generation we are on
        if(gps_ublox->_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
            if (!_send_message(gps_ublox, CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
                gps_ublox->_next_message--;
            }
        }
        break;
    case STEP_POLL_SBAS:
        if (gps._sbas_mode != SBAS_DoNotChange) {
            _send_message(gps_ublox, CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
        } else {
            gps_ublox->_unconfigured_messages &= ~CONFIG_SBAS;
        }
        break;
    case STEP_POLL_NAV:
        if (!_send_message(gps_ublox, CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_POLL_GNSS:
        if (!_send_message(gps_ublox, CLASS_CFG, MSG_CFG_GNSS, NULL, 0)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_POLL_TP5:
#if CONFIGURE_PPS_PIN
        if (!_send_message(gps_ublox, CLASS_CFG, MSG_CFG_TP5, NULL, 0)) {
            gps_ublox->_next_message--;
        }
#endif
        break;
    case STEP_NAV_RATE:
        if (!_send_message(gps_ublox, CLASS_CFG, MSG_CFG_RATE, NULL, 0)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_POSLLH:
        if (!_request_message_rate(gps_ublox, CLASS_NAV, MSG_POSLLH)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_STATUS:
        if (!_request_message_rate(gps_ublox, CLASS_NAV, MSG_STATUS)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_SOL:
        if (!_request_message_rate(gps_ublox, CLASS_NAV, MSG_SOL)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_VELNED:
        if (!_request_message_rate(gps_ublox, CLASS_NAV, MSG_VELNED)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_DOP:
       if (!_request_message_rate(gps_ublox, CLASS_NAV, MSG_DOP)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_MON_HW:
        if(!_request_message_rate(gps_ublox, CLASS_MON, MSG_MON_HW)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_MON_HW2:
        if(!_request_message_rate(gps_ublox, CLASS_MON, MSG_MON_HW2)) {
            gps_ublox->_next_message--;
        }
        break;
    case STEP_RAW:
#if UBLOX_RXM_RAW_LOGGING
        if(gps._raw_data == 0) {
            gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_RAW;
        } else if(!_request_message_rate(gps_ublox, CLASS_RXM, MSG_RXM_RAW)) {
            gps_ublox->_next_message--;
        }
#else
        gps_ublox->_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
        break;
    case STEP_RAWX:
#if UBLOX_RXM_RAW_LOGGING
        if(gps._raw_data == 0) {
            gps_ublox->_unconfigured_messages &= ~CONFIG_RATE_RAW;
        } else if(!_request_message_rate(gps_ublox, CLASS_RXM, MSG_RXM_RAWX)) {
            gps_ublox->_next_message--;
        }
#else
        gps_ublox->_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
        break;
    case STEP_VERSION:
        if(!gps_ublox->_have_version && !brd_get_soft_armed()) {
            _request_version(gps_ublox);
        } else {
            gps_ublox->_unconfigured_messages &= ~CONFIG_VERSION;
        }
        break;
    case STEP_TMODE:
        if (supports_F9_config(gps_ublox)) {
            if (!_configure_valget(gps_ublox, TMODE_MODE)) {
                gps_ublox->_next_message--;
            }
        }
        break;
    case STEP_RTK_MOVBASE:
#if GPS_MOVING_BASELINE
        if (supports_F9_config(gps_ublox)) {
        }
#endif
        break;
    case STEP_TIM_TM2:
#if UBLOX_TIM_TM2_LOGGING
        if(!_request_message_rate(gps_ublox, CLASS_TIM, MSG_TIM_TM2)) {
            gps_ublox->_next_message--;
        }
#else
        gps_ublox->_unconfigured_messages &= ~CONFIG_TIM_TM2;
#endif
        break;

    case STEP_M10: {
        if (gps_ublox->_hardware_generation == UBLOX_M10) {
            // special handling of M10 config
            const struct config_list *list = config_M10;
            const uint8_t list_length = ARRAY_SIZE(config_M10);
            UBLOX_DEBUG("Sending M10 settings");
            if (!_configure_config_set(gps_ublox, list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
                gps_ublox->_next_message--;
            }
        }
        break;
    }

    default:
        // this case should never be reached, do a full reset if it is hit
        gps_ublox->_next_message = STEP_PVT;
        break;
    }
}


/*
 *  send a ublox message
 */
static bool _send_message(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size)
{
    if (SerialManager_tx_space(gps_ublox->backend.port) < (sizeof(struct ubx_header) + 2 + size)) {
        return false;
    }
    struct ubx_header header;
    uint8_t ck_a=0, ck_b=0;
    header.preamble1 = PREAMBLE1;
    header.preamble2 = PREAMBLE2;
    header.msg_class = msg_class;
    header.msg_id    = msg_id;
    header.length    = size;

    _update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, &ck_a, &ck_b);
    _update_checksum((uint8_t *)msg, size, &ck_a, &ck_b);

    SerialManager_write(gps_ublox->backend.port, (const uint8_t *)&header, sizeof(header));
    SerialManager_write(gps_ublox->backend.port, (const uint8_t *)msg, size);
    SerialManager_write(gps_ublox->backend.port, (const uint8_t *)&ck_a, 1);
    SerialManager_write(gps_ublox->backend.port, (const uint8_t *)&ck_b, 1);
    return true;
}

/*
 *  update checksum for a set of bytes
 */
static void _update_checksum(uint8_t *data, uint16_t len, uint8_t *ck_a, uint8_t *ck_b)
{
    while (len--) {
        (*ck_a) += (*data);
        (*ck_b) += (*ck_a);
        data++;
    }
}

/*
 *  requests the given message rate for a specific message class
 *  and msg_id
 *  returns true if it sent the request, false if waiting on knowing the port
 */
static bool _request_message_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id)
{
    // Without knowing what communication port is being used it isn't possible to verify
    // always ensure we have a port before sending the request
    if(gps_ublox->_ublox_port >= UBLOX_MAX_PORTS) {
        _request_port(gps_ublox);
        return false;
    } else {
        struct ubx_cfg_msg msg;
        msg.msg_class = msg_class;
        msg.msg_id    = msg_id;
        return _send_message(gps_ublox, CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
    }
}

// Requests the ublox driver to identify what port we are using to communicate
static void _request_port(sensor_gps_ublox_t gps_ublox)
{
    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+2)) {
        // not enough space - do it next time
        return;
    }
    _send_message(gps_ublox, CLASS_CFG, MSG_CFG_PRT, NULL, 0);
}

static void _request_version(sensor_gps_ublox_t gps_ublox)
{
    _send_message(gps_ublox, CLASS_MON, MSG_MON_VER, NULL, 0);
}

static void _configure_rate(sensor_gps_ublox_t gps_ublox)
{
    struct ubx_cfg_nav_rate msg;
    // require a minimum measurement rate of 5Hz
    msg.measure_rate_ms = sensor_gps_get_rate_ms(gps_ublox->backend.state->instance);
    msg.nav_rate        = 1;
    msg.timeref         = 0;     // UTC time
    _send_message(gps_ublox, CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
}

// return true if GPS is capable of F9 config
static bool supports_F9_config(sensor_gps_ublox_t gps_ublox)
{
    return gps_ublox->_hardware_generation == UBLOX_F9 || gps_ublox->_hardware_generation == UBLOX_M10;
}

/*
 *  configure F9 based key/value pair - VALSET
 */
static bool _configure_valset(sensor_gps_ublox_t gps_ublox, enum ConfigKey key, const void *value, uint8_t layers)
{
    if (!supports_F9_config(gps_ublox)) {
        return false;
    }
    const uint8_t len = config_key_size(key);
    struct ubx_cfg_valset msg = {0};
    uint8_t buf[sizeof(msg)+len];
    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
        return false;
    }
    msg.version = 1;
    msg.layers = 7; // all layers
    msg.transaction = 0;
    msg.key = (uint32_t)(key);
    rt_memcpy(buf, &msg, sizeof(msg));
    rt_memcpy(&buf[sizeof(msg)], value, len);
    bool ret = _send_message(gps_ublox, CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf));
    return ret;
}

/*
 *  configure F9 based key/value pair - VALGET
 */
static bool _configure_valget(sensor_gps_ublox_t gps_ublox, enum ConfigKey key)
{
    if (!supports_F9_config(gps_ublox)) {
        return false;
    }
    struct {
        struct ubx_cfg_valget msg;
        enum ConfigKey key;
    } msg = {0};

    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) {
        return false;
    }
    msg.msg.version = 0;
    msg.msg.layers = 0; // ram
    msg.key = key;
    return _send_message(gps_ublox, CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
}

/*
 *  configure F9 based key/value pair for a complete configuration set
 *
 *  this method requests each configuration variable from the GPS.
 *  When we handle the reply in _parse_gps we may then choose to set a
 *  MSG_CFG_VALSET back to the GPS if we don't like its response.
 */
static bool _configure_config_set(sensor_gps_ublox_t gps_ublox, const struct config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers)
{
    gps_ublox->active_config.list = list;
    gps_ublox->active_config.count = count;
    gps_ublox->active_config.done_mask = 0;
    gps_ublox->active_config.unconfig_bit = unconfig_bit;
    gps_ublox->active_config.layers = layers;
    // we start by fetching multiple values at once (with fetch_index
    // -1) then if we get a NACK for VALGET we switch to fetching one
    // value at a time. This copes with the M10S which can only fetch
    // one value at a time
    gps_ublox->active_config.fetch_index = -1;

    uint8_t buf[sizeof(struct ubx_cfg_valget)+count*sizeof(enum ConfigKey)];
    struct ubx_cfg_valget msg  = {0};
    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
        return false;
    }
    msg.version = 0;
    msg.layers = 0; // ram
    rt_memcpy(buf, &msg, sizeof(msg));
    for (uint8_t i=0; i<count; i++) {
        rt_memcpy(&buf[sizeof(msg)+i*sizeof(enum ConfigKey)], &list[i].key, sizeof(enum ConfigKey));
    }
    return _send_message(gps_ublox, CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
}

/*
 * save gps configurations to non-volatile memory sent until the call of
 * this message
 */
static void _save_cfg(sensor_gps_ublox_t gps_ublox)
{
    struct ubx_cfg_cfg save_cfg;
    save_cfg.clearMask = 0;
    save_cfg.saveMask = SAVE_CFG_ALL;
    save_cfg.loadMask = 0;
    _send_message(gps_ublox, CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
    gps_ublox->_last_cfg_sent_time = time_millis();
    gps_ublox->_num_cfg_save_tries++;
    gcs_send_text(MAV_SEVERITY_INFO,
                                     "GPS %d: u-blox saving config",
                                     gps_ublox->backend.state->instance + 1);
}

static void _verify_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
    uint8_t desired_rate;
    uint32_t config_msg_id;

    switch(msg_class) {
    case CLASS_NAV:
        switch(msg_id) {
        case MSG_POSLLH:
            desired_rate = gps_ublox->havePvtMsg ? 0 : RATE_POSLLH;
            config_msg_id = CONFIG_RATE_POSLLH;
            break;
        case MSG_STATUS:
            desired_rate = gps_ublox->havePvtMsg ? 0 : RATE_STATUS;
            config_msg_id = CONFIG_RATE_STATUS;
            break;
        case MSG_SOL:
            desired_rate = gps_ublox->havePvtMsg ? 0 : RATE_SOL;
            config_msg_id = CONFIG_RATE_SOL;
            break;
        case MSG_PVT:
            desired_rate = RATE_PVT;
            config_msg_id = CONFIG_RATE_PVT;
            break;
        case MSG_TIMEGPS:
            desired_rate = RATE_TIMEGPS;
            config_msg_id = CONFIG_RATE_TIMEGPS;
            break;
        case MSG_VELNED:
            desired_rate = gps_ublox->havePvtMsg ? 0 : RATE_VELNED;
            config_msg_id = CONFIG_RATE_VELNED;
            break;
        case MSG_DOP:
            desired_rate = RATE_DOP;
            config_msg_id = CONFIG_RATE_DOP;
            break;
        default:
            return;
        }
        break;
    case CLASS_MON:
        switch(msg_id) {
        case MSG_MON_HW:
            desired_rate = RATE_HW;
            config_msg_id = CONFIG_RATE_MON_HW;
            break;
        case MSG_MON_HW2:
            desired_rate = RATE_HW2;
            config_msg_id = CONFIG_RATE_MON_HW2;
            break;
        default:
            return;
        }
        break;
#if UBLOX_RXM_RAW_LOGGING
    case CLASS_RXM:
        switch(msg_id) {
        case MSG_RXM_RAW:
            desired_rate = gps._raw_data;
            config_msg_id = CONFIG_RATE_RAW;
            break;
        case MSG_RXM_RAWX:
            desired_rate = gps._raw_data;
            config_msg_id = CONFIG_RATE_RAW;
            break;
        default:
            return;
        }
        break;
#endif // UBLOX_RXM_RAW_LOGGING
#if UBLOX_TIM_TM2_LOGGING
    case CLASS_TIM:
        if (msg_id == MSG_TIM_TM2) {
            desired_rate = RATE_TIM_TM2;
            config_msg_id = CONFIG_TIM_TM2;
            break;
        }
        return;
#endif // UBLOX_TIM_TM2_LOGGING
    default:
        return;
    }

    if (rate == desired_rate) {
        // coming in at correct rate; mark as configured
        gps_ublox->_unconfigured_messages &= ~config_msg_id;
        return;
    }

    // coming in at wrong rate; try to configure it
    _configure_message_rate(gps_ublox, msg_class, msg_id, desired_rate);
    gps_ublox->_unconfigured_messages |= config_msg_id;
    gps_ublox->_cfg_needs_save = true;
}

/*
 *  configure a UBlox GPS for the given message rate for a specific
 *  message class and msg_id
 */
static bool _configure_message_rate(sensor_gps_ublox_t gps_ublox, uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
    if (SerialManager_tx_space(gps_ublox->backend.port) < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
        return false;
    }

    struct ubx_cfg_msg_rate msg;
    msg.msg_class = msg_class;
    msg.msg_id    = msg_id;
    msg.rate      = rate;
    return _send_message(gps_ublox, CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
}

// return size of a config key, or 0 if unknown
static uint8_t config_key_size(enum ConfigKey key)
{
    // step over the value
    const uint8_t key_size = ((uint32_t)(key) >> 28) & 0x07; // mask off the storage size
    switch (key_size) {
    case 0x1: // bit
    case 0x2: // byte
        return 1;
    case 0x3: // 2 bytes
        return 2;
    case 0x4: // 4 bytes
        return 4;
    case 0x5: // 8 bytes
        return 8;
    default:
        break;
    }
    // invalid
    return 0;
}

static void log_mon_hw(sensor_gps_ublox_t gps_ublox)
{
#if HAL_LOGGING_ENABLED

#endif
}

static void log_mon_hw2(sensor_gps_ublox_t gps_ublox)
{
#if HAL_LOGGING_ENABLED

#endif
}

static void unexpected_message(sensor_gps_ublox_t gps_ublox)
{
    UBLOX_DEBUG("Unexpected message 0x%02x 0x%02x", (unsigned)gps_ublox->_class, (unsigned)gps_ublox->_msg_id);
    if (++gps_ublox->_disable_counter == 0) {
        // disable future sends of this message id, but
        // only do this every 256 messages, as some
        // message types can't be disabled and we don't
        // want to get into an ack war
        UBLOX_DEBUG("Disabling message 0x%02x 0x%02x", (unsigned)gps_ublox->_class, (unsigned)gps_ublox->_msg_id);
        _configure_message_rate(gps_ublox, gps_ublox->_class, gps_ublox->_msg_id, 0);
    }
}

#if UBLOX_RXM_RAW_LOGGING
static void log_rxm_raw(const struct ubx_rxm_raw *raw)
{
#if HAL_LOGGING_ENABLED

#endif
}

static void log_rxm_rawx(const struct ubx_rxm_rawx *raw)
{
#if HAL_LOGGING_ENABLED

#endif
}
#endif // UBLOX_RXM_RAW_LOGGING

// uBlox specific check_new_itow(), handling message length
static void _check_new_itow(sensor_gps_ublox_t gps_ublox, uint32_t itow)
{
    sensor_gps_backend_check_new_itow(&gps_ublox->backend, itow, gps_ublox->_payload_length + sizeof(struct ubx_header) + 2);
}

/*
  find index of a config key in the active_config list, or -1
 */
static int8_t find_active_config_index(sensor_gps_ublox_t gps_ublox, enum ConfigKey key)
{
#if GPS_MOVING_BASELINE
    if (gps_ublox->active_config.list == NULL) {
        return -1;
    }
    for (uint8_t i=0; i<gps_ublox->active_config.count; i++) {
        if (key == gps_ublox->active_config.list[i].key) {
            return (int8_t)i;
        }
    }
#endif
    return -1;
}

/*------------------------------------test------------------------------------*/
#endif


